/**
 * @file motor.c
 * @brief 电机控制
 *
 * @author 钟涛
 *
 * @date 2022-05-08
 *
 * @copyright 海南大学 - 追光者
 */

#include <rtthread.h>
#include <rtdevice.h>
#include "drv_common.h"
#include "app_main.h"

#define DBG_TAG "motor"
#define DBG_LVL DBG_INFO
#include <rtdbg.h>

/**
 *  PWM周期，单位为纳秒
 */
#define PWM3_PERIOD          30000
#define STEER_PERIOD         20000000

extern TIM_HandleTypeDef STEER_UD_HTIM;
extern TIM_HandleTypeDef STEER_LR_HTIM;
extern float yaw;

bool is_backward = false;
bool is_distance_stop = false;

/**
 * 方向PID参数
 */
float dir_kp = 9;
float dir_ki = 1;

/**
 *  PWM设备句柄
 */
static struct rt_device_pwm *pwm3_dev;

/**
 * pid控制
 */
uint16_t PID(int16_t traget, int16_t previous)
{
    uint16_t pwm_val1;
    int16_t e;
    static int16_t E;

    e = traget - previous;
    E = E + e;
    //if (E > 100)E = 80;
    //if (E < -100)E = -80;
    pwm_val1 = dir_kp * (e) + dir_ki * (E) + 20000;
    //if (pwm_val1 > 24300)pwm_val1 = 24300;
    //if (pwm_val1 < 23700)pwm_val1 = 23700;

    return pwm_val1;
}

void pid_thread(void *params)
{
    while (1)
    {
        rt_thread_mdelay(20);
    }
}

/**
 * @brief 向前无延时
 */
void Forward(void)
{
    if (is_distance_stop)
        return;

    rt_pin_write(AIN1_PIN, PIN_HIGH);
    rt_pin_write(AIN2_PIN, PIN_HIGH);
    rt_pin_write(BIN1_PIN, PIN_HIGH);
    rt_pin_write(BIN2_PIN, PIN_HIGH);
    rt_pwm_set(pwm3_dev, MOTOR_DEV_CHANNEL1, PWM3_PERIOD, 24000);
    rt_pwm_set(pwm3_dev, MOTOR_DEV_CHANNEL2, PWM3_PERIOD, 24700);
}
MSH_CMD_EXPORT(Forward, Forward);

/**
 * @brief 向后无延时
 */
void Backward(void)
{
    LOG_D("Backward");
    rt_pin_write(AIN1_PIN, PIN_HIGH);
    rt_pin_write(AIN2_PIN, PIN_LOW);
    rt_pin_write(BIN1_PIN, PIN_HIGH);
    rt_pin_write(BIN2_PIN, PIN_LOW);
    rt_pwm_set(pwm3_dev, MOTOR_DEV_CHANNEL1, PWM3_PERIOD, 24500);
    rt_pwm_set(pwm3_dev, MOTOR_DEV_CHANNEL2, PWM3_PERIOD, 24500);
    is_backward = true;
    is_distance_stop = false;
}
MSH_CMD_EXPORT(Backward, Backward);

/**
 * @brief 向右无延时
 */
void Rightward(void)
{
    if (is_distance_stop)
        return;

    rt_pin_write(AIN1_PIN, PIN_HIGH);
    rt_pin_write(AIN2_PIN, PIN_HIGH);
    rt_pin_write(BIN1_PIN, PIN_LOW);
    rt_pin_write(BIN2_PIN, PIN_LOW);
    rt_pwm_set(pwm3_dev, MOTOR_DEV_CHANNEL1, PWM3_PERIOD, 22500);
    rt_pwm_set(pwm3_dev, MOTOR_DEV_CHANNEL2, PWM3_PERIOD, 22500);
}
MSH_CMD_EXPORT(Rightward, Rightward);

/**
 * @brief 向左无延时
 */
void Leftward(void)
{
    if (is_distance_stop)
        return;

    rt_pin_write(AIN1_PIN, PIN_LOW);
    rt_pin_write(AIN2_PIN, PIN_LOW);
    rt_pin_write(BIN1_PIN, PIN_HIGH);
    rt_pin_write(BIN2_PIN, PIN_HIGH);
    rt_pwm_set(pwm3_dev, MOTOR_DEV_CHANNEL1, PWM3_PERIOD, 22500);
    rt_pwm_set(pwm3_dev, MOTOR_DEV_CHANNEL2, PWM3_PERIOD, 22500);
}
MSH_CMD_EXPORT(Leftward, Leftward);

void Stop(void)
{
    rt_pin_write(AIN1_PIN, PIN_LOW);
    rt_pin_write(AIN2_PIN, PIN_LOW);
    rt_pin_write(BIN1_PIN, PIN_LOW);
    rt_pin_write(BIN2_PIN, PIN_LOW);
    rt_pwm_set(pwm3_dev, MOTOR_DEV_CHANNEL1, PWM3_PERIOD, 0);
    rt_pwm_set(pwm3_dev, MOTOR_DEV_CHANNEL2, PWM3_PERIOD, 0);
    is_backward = false;
}
MSH_CMD_EXPORT(Stop, Leftward);

void distance_stop(void)
{
    if (!is_backward && is_distance_stop == false)
    {
        LOG_W("distance_stop");
        Stop();
        is_distance_stop = true;
    }
}

int pwm = 1000;

void pwm_add(void)
{
    pwm += 20;
    rt_pwm_set(pwm3_dev, MOTOR_DEV_CHANNEL1, PWM3_PERIOD, pwm); // 左轮
    LOG_I("pwm: %d", pwm);
}
MSH_CMD_EXPORT(pwm_add, pwm_add);

void pwm_reduce(void)
{
    pwm -= 20;
    LOG_I("pwm: %d", pwm);
    rt_pwm_set(pwm3_dev, MOTOR_DEV_CHANNEL1, PWM3_PERIOD, pwm); // 左轮
}
MSH_CMD_EXPORT(pwm_reduce, pwm_reduce);

int pwm_camera_ud = 1000;
const int pwm_camera_ud_min = 800;
const int pwm_camera_ud_middle = 1600;
const int pwm_camera_ud_max = 2000;

void camera_uu(void)
{
    if (pwm_camera_ud > pwm_camera_ud_min)
        pwm_camera_ud -= 5;
    __HAL_TIM_SET_COMPARE(&STEER_UD_HTIM, STEER_UD_CHANNEL, pwm_camera_ud); // 单位us
    LOG_D("pwm_camera_ud: %d", pwm_camera_ud);
}
MSH_CMD_EXPORT(camera_uu, camera_uu);

void camera_dd(void)
{
    if (pwm_camera_ud < pwm_camera_ud_max)
        pwm_camera_ud += 5;
    __HAL_TIM_SET_COMPARE(&STEER_UD_HTIM, STEER_UD_CHANNEL, pwm_camera_ud); // 单位us
    LOG_D("pwm_camera_ud: %d", pwm_camera_ud);
}
MSH_CMD_EXPORT(camera_dd, camera_dd);

void camera_ud_middle(void)
{
    pwm = pwm_camera_ud_middle;
    __HAL_TIM_SET_COMPARE(&STEER_UD_HTIM, STEER_UD_CHANNEL, pwm); // 单位us
}

void camera_up(void)
{
    pwm = pwm_camera_ud_min;
    __HAL_TIM_SET_COMPARE(&STEER_UD_HTIM, STEER_UD_CHANNEL, pwm); // 单位us
}
MSH_CMD_EXPORT(camera_up, camera_up);

void camera_down(void)
{
    pwm = pwm_camera_ud_max;
    __HAL_TIM_SET_COMPARE(&STEER_UD_HTIM, STEER_UD_CHANNEL, pwm); // 单位us
}
MSH_CMD_EXPORT(camera_down, camera_down);

int pwm_camera_lr = 1600;
const int pwm_camera_lr_min = 500;
const int pwm_camera_lr_middle = 1500;
const int pwm_camera_lr_max = 2500;

void camera_rr(void)
{
    if (pwm_camera_lr > pwm_camera_lr_min)
        pwm_camera_lr -= 5;
    __HAL_TIM_SET_COMPARE(&STEER_LR_HTIM, STEER_LR_CHANNEL, pwm_camera_lr); // 单位us
    LOG_D("pwm_camera_lr: %d", pwm_camera_lr);
}

void camera_ll(void)
{
    if (pwm_camera_lr < pwm_camera_lr_max)
        pwm_camera_lr += 5;
    __HAL_TIM_SET_COMPARE(&STEER_LR_HTIM, STEER_LR_CHANNEL, pwm_camera_lr); // 单位us
    LOG_D("pwm_camera_lr: %d", pwm_camera_lr);
}
MSH_CMD_EXPORT(camera_ll, camera_ll);
MSH_CMD_EXPORT(camera_rr, camera_rr);

void rigin(void);

void camera_left(void)
{
    __HAL_TIM_SET_COMPARE(&STEER_LR_HTIM, STEER_LR_CHANNEL, pwm_camera_lr_max); // 单位us
}
MSH_CMD_EXPORT(camera_left, camera_left);

void camera_lr_middle(void)
{
    __HAL_TIM_SET_COMPARE(&STEER_LR_HTIM, STEER_LR_CHANNEL, pwm_camera_lr_middle); // 单位us
}
MSH_CMD_EXPORT(camera_lr_middle, camera_lr_middle); // 导出命令（函数名，介绍）

void camera_right(void)
{
    __HAL_TIM_SET_COMPARE(&STEER_LR_HTIM, STEER_LR_CHANNEL, pwm_camera_lr_min); // 单位us
}
MSH_CMD_EXPORT(camera_right, camera_right);

void TIM_SetTIM14Compare1(uint32_t compare)
{
    TIM14->CCR1 = compare;
}

int motor_init(void)
{

    pwm3_dev = (struct rt_device_pwm *) rt_device_find(MOTOR_DEV_NAME);
    rt_pwm_enable(pwm3_dev, MOTOR_DEV_CHANNEL1);
    rt_pwm_enable(pwm3_dev, MOTOR_DEV_CHANNEL2);
    rt_pwm_set(pwm3_dev, MOTOR_DEV_CHANNEL1, PWM3_PERIOD, 0);
    rt_pwm_set(pwm3_dev, MOTOR_DEV_CHANNEL2, PWM3_PERIOD, 0);

    HAL_TIM_PWM_Start(&STEER_UD_HTIM, STEER_UD_CHANNEL);
    HAL_TIM_PWM_Start(&STEER_LR_HTIM, STEER_LR_CHANNEL);

    // 初始转角
    __HAL_TIM_SET_COMPARE(&STEER_UD_HTIM, STEER_UD_CHANNEL, 1000);
    __HAL_TIM_SET_COMPARE(&STEER_LR_HTIM, STEER_LR_CHANNEL, 1000);
    camera_ud_middle();
    camera_lr_middle();

    rt_pin_mode(AIN1_PIN, PIN_MODE_OUTPUT);
    rt_pin_mode(BIN1_PIN, PIN_MODE_OUTPUT);
    rt_pin_mode(AIN2_PIN, PIN_MODE_OUTPUT);
    rt_pin_mode(BIN2_PIN, PIN_MODE_OUTPUT);

    return 0;
}
INIT_DEVICE_EXPORT(motor_init);
